'''
Description:          
Author: WANG Hao
Date: 2022-09-26 10:55:38
LastEditTime: 2022-09-26 11:36:22
LastEditors: WANG Hao
'''
#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@Copyright:  Jihua Lab 2022.
@File    :   sea_dual_arm_contact----.py    
@Author :   HaoWANG, Foshan，China
@Contact :   wanghao@jihualab.com
@License :   JHL ( not BSD)

@Description:  



@Create Time   :   2022-09-26 15:38:12
@Modify Time      @Author    @Version    @Desciption
------------      -------    --------    -----------
2022-09-20 15:38:12  wanghao      1.1.0         None
20220920 
20220920 TODO: Three Threads: 1 - Dual arm motion control 
                2 - records the left and right robot arm pitch joint servo motore angle state 
                3- records the right pitch joint angle state  - SEA -0
                4- records the left pitch joint angle state    -  SEA -1
20220926 indoor and outdoor SEA test
'''

# import ADS1263 lib
# from asyncio.windows_events import NULL
from concurrent.futures import thread
# from ctypes.wintypes import SIZE
from subprocess import call
import time
import ADS1263
import queue

# import dual arm motion control lib
import math
import logging
import threading

import uservo
import serial

from MotionGroup import MotionGroup
from ServoPose import Pose
from wrist_gripper import WristGripperControl
from robot_arm import ArmMotionControl

"""
controller paramters
"""
# 标志
exitFlag = 0
recordFlag = 0

# Global const para
THREAD_MODE_CONTROL = ["control", "Control", "CONTROL"]
THREAD_MODE_RECORD = ["record", "Record", "Records", "RECORD", "RECORDS"]
THREAD_MODE_RECORD_SEA =[ "record_sea_0", "record_sea_1"]

SHORT_TIME_DELAY = 0.02  # time delay para
MEDIA_TIME_DELAY = 0.10
LONG_TIME_DELAY = 1.0

# recording
SAMPLING_FREQUENCY = 10  # units: Hz
fileName = "robot_arm_joint_records.txt"

ADC_Val_buffer = queue.Queue(10)

# servo motor velocity： mean rotation speed （deg/sec）  
VEL = {'VERY_LOW_SPEED': 10, 'LOW_SPEED': 15, 'MID_SPEED': 30, 'HIGH_SPEED': 60}
# Control period : total time used for one servo motor control cmd,( ms) 
CONTROL_PERIOD = {"SEC_5":5000, "SEC_3":3000, "SEC_2":2000, "SEC_1.6":1600, "SEC_1.8":1800, "SEC_1.4":1400, "SEC_1.2":1200, "SEC_1.0":1000}
CONTROL_MODE = {'Teach': True, 'Repeat': False}
# right arm + right wrist + left and right gripper angle

"""
INIT POSE - INIT joint state "I_"
"""
DUAL_ARM_INIT_POSE = [-151.0, 138.0, 110.0, 0, 
                        0.0, 23.0, 72.0, 0, 
                        10, 10]
"""
START POSE - TEST START joint state "I"
"""
# DUAL_ARM_START_POSE = [-151.0, 138.0, -26.0+30, 0, 
#                         0.0, 23.0,-60.0 + 30, 0, 
#                         30, 30]

# 小臂伸直 Joint =-26/ -60, 弯曲90度 joint =110/72 

delta_theta  = 62                 # right arm shoulder pithch joint rotation angle (delta theta form start pose)

DUAL_ARM_START_POSE = [-151.0, 138.0, 110, 0, 
                        0.0, 23.0,72, 0, 
                        30, 30]

DUAL_ARM_TARGET_POSE = [-151.0, 138.0, -26, 85, 
                        0.0, 23.0, -60, 86, 
                        65, 72]
ending_servo_idx = [0, 1, 2, 3]
"""
END OF controller paramters
"""

# Global Var
THETA_STATIC = [19, 174.0]                   # init  the SEA Theta val

DELTA_THETA_THRESHOLD = [25.0, 25.0]            # delta_theta_t = | theta_sea_t - THETA_STATIC    |   <=  DELTA_THETA_THRESHOLD, in deg

current_pose_t = [0.0, 0.0 ]  # current dual arm joint pose of serial servo 1 - L pitch ; 5 - R pitch

theta_sea_t = [0.0, 0.0 ]              #left - ID0 / right - ID1 , SEA current theta deg



class dfrThread(threading.Thread):
    def __init__(self, threadID, name, controlPeriod, samplingFrequency, dual_arm_obj, wrist_gripper = 0, sea_serial = 0):
        threading.Thread.__init__(self)
        self.threadID = threadID
        self.threadName = name
        self.controlPeriod = controlPeriod
        self.samplingFrequency = samplingFrequency
        self.dual_arm_obj = dual_arm_obj
        self.wrist_gripper = wrist_gripper
        self.sea_serial = sea_serial

    def run(self):
        print("开始线程：" + self.threadName)
        if (self.threadName in THREAD_MODE_CONTROL):
            logging.info("%s: %s" % (self.threadName, time.ctime(time.time())))
            robot_arm_control(self.threadName, self.dual_arm_obj, self.wrist_gripper, self.controlPeriod)

        elif self.threadName in THREAD_MODE_RECORD:
            record_servo_info(self.threadName, self.dual_arm_obj, samplingFrequency=SAMPLING_FREQUENCY)
        
        elif self.threadName in THREAD_MODE_RECORD_SEA:
            record_sea_info(threadName=self.threadName, sea_serial=self.sea_serial)
            
        else:
            logging.error("---- Logging Interrupt----: self.threadName should be in THREAD_MODE_RECORD or THREAD_MODE_CONTROL !")

        print("退出线程：" + self.name)


"""
Description：
1. robot arm motion control module for dual-arm SEA joint puse reaction experiments
2. threads for motion control and data records function 
3. PARAMTERS tune for CONTROL PERIOD and VEL\ACC\DEC time  
"""
def robot_arm_control(threadName, dual_arm, wrist_gripper, controlPeriod=3000, vel = VEL.get('VERY_LOW_SPEED')):
    # dual arm motion control threading
    dual_arm_servo = dual_arm.get_arm_servo_list()
    record_serial = 1

    while dual_arm and (threadName in THREAD_MODE_CONTROL):
        keyInput = input('---- Logging Info----:  WaitKey to continue input (c or x):')
        if keyInput == 'x':
            # gripper control
            wrist_gripper.set_servo_angle(servo_idx=1, angle=DUAL_ARM_INIT_POSE[8])
            wrist_gripper.set_servo_angle(servo_idx=3, angle=DUAL_ARM_INIT_POSE[9])
            dual_arm.set_all_servo_mode(CONTROL_MODE="CIRCLE")
            time.sleep(1)
            wrist_gripper.set_servo_angle(servo_idx=1, angle=DUAL_ARM_TARGET_POSE[8])
            wrist_gripper.set_servo_angle(servo_idx=3, angle=DUAL_ARM_TARGET_POSE[9])
            logging.warning('---- Logging Info----:  The waitKey input (c or x): {}'.format(keyInput))
            break
        elif keyInput == 'c':
            # dual_arm再现
            # move the left and right robot arm to start pose for later trajectory control
            for SERVO_ID in dual_arm_servo:
                dual_arm.set_single_servo_pose(
                    Pose=generate_pose_vtc(angle=DUAL_ARM_START_POSE[SERVO_ID], vel=vel,
                                           iterval=controlPeriod,
                                           t_acc=80,
                                           t_dec=50, power=10000, mean_dps=20, servo_id=SERVO_ID))
            logging.info('---- Logging Info----:  Pre to Move Time delay: {}  sec'.format(5))
            time.sleep(5 * LONG_TIME_DELAY)

            # 2022-09-20 contact force control
            # start pose trajectory control
            for i in range(3):
                # target pose trajectory control for left arm servo id = 0,1,2
                for SERVO_ID in dual_arm_servo:
                    dual_arm.set_single_servo_pose(
                        Pose=generate_pose_vtc(angle=DUAL_ARM_TARGET_POSE[SERVO_ID],
                                            vel=vel,
                                            iterval=controlPeriod,
                                            t_acc=80,
                                            t_dec=50, power=10000, mean_dps=20, servo_id=SERVO_ID))
                time.sleep(4 * LONG_TIME_DELAY)

                for SERVO_ID in dual_arm_servo:
                    dual_arm.set_single_servo_pose(
                    Pose=generate_pose_vtc(angle=DUAL_ARM_START_POSE[SERVO_ID], vel=vel,
                                           iterval=controlPeriod,
                                           t_acc=80,
                                           t_dec=50, power=10000, mean_dps=20, servo_id=SERVO_ID))
                time.sleep(4*LONG_TIME_DELAY)
                
            #TODO contact force control and sea force threshold
            # current_pose_t[0], current_pose_t[1]
            
            delta_theta =  [math.fabs(theta_sea_t[0] - THETA_STATIC[0]),
                                    math.fabs(theta_sea_t[1] - THETA_STATIC[1]) ]

            if delta_theta[0]>DELTA_THETA_THRESHOLD[0] or delta_theta[1] >DELTA_THETA_THRESHOLD[1]:
                pose_current = [DUAL_ARM_TARGET_POSE[0], current_pose_t[0], DUAL_ARM_TARGET_POSE[2], DUAL_ARM_TARGET_POSE[3],
                                                    DUAL_ARM_TARGET_POSE[4], current_pose_t[1], DUAL_ARM_TARGET_POSE[6], DUAL_ARM_TARGET_POSE[7],
                                                    DUAL_ARM_TARGET_POSE[8], DUAL_ARM_TARGET_POSE[9]]
                for SERVO_ID in dual_arm_servo:
                    dual_arm.set_single_servo_pose(
                        Pose=generate_pose_vtc(angle=pose_current[SERVO_ID], vel=vel,
                                           iterval=controlPeriod,
                                           t_acc=80,
                                           t_dec=50, power=10000, mean_dps=30, servo_id=SERVO_ID))

            # while True:
            #     loadingKeyInput = input('waitKey to control robot arm and read joint state -> input (y or n):')
            #     if loadingKeyInput == 'n':
            #         # 修改标志位，线程上锁
            #         # threadLock.acquire()
            #         break
            #     elif loadingKeyInput == 'y':
            #         # threadLock.acquire()
            #         recordFlag = 1  # record 0->1
            #         exitFlag = 0  # exit 0->0
            #         # threadLock.release()
                    
            #         #TODO: records shoulder pitch joint servo motor angle in DEGREE 
            #         # file_joint_angle = "file_joint_angle_records"+record_serial+".txt"
            #         # with open(file_joint_angle, 'w') as f:
            #         #     f.open

            #         # target pose trajectory control for right arm servo id = 0,1,2,4,5,6
            #         # time.sleep(1)
        else:
            logging.warning('---- Logging Info----:  WaitKey input (c or x): {}'.format(keyInput))

# 1. servo motor real rotation state records\
# 2. record robot arm servo info and SEA joint angle state infomation
def record_servo_info(threadName, dual_arm, samplingFrequency, frame_num = 3000):
    while dual_arm and samplingFrequency and (threadName in THREAD_MODE_RECORD):
        loadingKeyInput = input('---- Logging info----:  WaitKey to record continue input (y or n):')
        if loadingKeyInput == 'n':
            threadName.exit()
            logging.info("---- Logging Interrupt----: exitFlag = True: threadName.exit() !")
            break

        elif loadingKeyInput == 'y':
            with open(fileName, 'w') as f:
                for frame in range(frame_num):
                    left_pitch_temp = dual_arm.read_single_servo_angle(SERVO_ID=2) -DUAL_ARM_START_POSE[2]
                    right_pitch_temp = dual_arm.read_single_servo_angle(SERVO_ID=6) - DUAL_ARM_START_POSE[6]
                    current_pose_t = [left_pitch_temp, right_pitch_temp]
                    # data file write as txt
                    f.write("frame ID ： {}   {}   {}   {} ".format(frame,
                                                            time.ctime(time.time()),
                                                            left_pitch_temp,
                                                           right_pitch_temp) + '\r')
                    time.sleep(1 / samplingFrequency)
            break
        # logging.info("%s: %s" % (threadName, time.ctime(time.time())))
        # recoding the angle data and the power data


def get_sea_val():

    REF = 5.08  # Modify according to actual voltage
    # external AVDD and AVSS(Default), or internal 2.5V
    #servo_list = [0, 1, 4, 5]  # 0,1 left arm ;4,5 right arm shoulder joint yaw and pitch, respectively
    #SEA_Balanced_Angle = []  # static information of sea joint balanced angle
    # def average size

    try:
        ADC = ADS1263.ADS1263()
        if (ADC.ADS1263_init_ADC1('ADS1263_7200SPS') == -1):
            exit()
        ADC.ADS1263_SetMode(0)

        # ADC.ADS1263_DAC_Test(1, 1)      # Open IN6
        # ADC.ADS1263_DAC_Test(0, 1)      # Open IN7

        if (ADC.ADS1263_init_ADC2('ADS1263_ADC2_400SPS') == -1):
            exit()

        logging.info("---- Get SEA data Success !----- ")
        while (1):
            ADC_Value = ADC.ADS1263_GetAll_ADC2()  # get ADC2 value
            ADC_Val_buffer.put(ADC_Value)

    except IOError as e:
        logging.error(e)

    except KeyboardInterrupt:
        logging.info("---- Logging Interrupt----:  ctrl + c: Program end")
        # logging.info(current_yaw_right_buffer,current_pitch_right_buffer)
        logging.info("---- Logging Interrupt ----:  sea_serial NOT in DefaultSEAChannelSer: threadName.exit() !")
        ADC.ADS1263_Exit()
        threadName.exit()
        exit()

# record robot arm servo info and SEA joint angle state infomation
def record_sea_info(threadName, 
                    sea_serial, 
                    samplingFrequency = 30, 
                    auto_start = True):

    DefaultSEAChannelSer = [0 , 1]
  
    REF = 5.08  # Modify according to actual voltage
    # external AVDD and AVSS(Default), or internal 2.5V
    #servo_list = [0, 1, 4, 5]  # 0,1 left arm ;4,5 right arm shoulder joint yaw and pitch, respectively
    #SEA_Balanced_Angle = []  # static information of sea joint balanced angle
    # def average size

    kernel_size = 2
    frequency = samplingFrequency   # default 20 Hz
    moving_average_a = MovingAverage(kernel_size)
    moving_average_b = MovingAverage(kernel_size)

    current_pitch_right_buffer = []  # channel 1
    current_pitch_left_buffer = []  # channel 0

    if auto_start and sea_serial in DefaultSEAChannelSer:     
        # sea_serial - > channel 0, 1, and 0 for left shoulder pitchjoint
        #                 channel 1 for right shoulder pitch joint SEA state estimation results record 
        if sea_serial == 1:
            # queue data get thread
            cnt =  0
            call_times = 0
            # try:
                # ADC = ADS1263.ADS1263()
                # if (ADC.ADS1263_init_ADC1('ADS1263_7200SPS') == -1):
                #     exit()
                # ADC.ADS1263_SetMode(0)

                # # ADC.ADS1263_DAC_Test(1, 1)      # Open IN6
                # # ADC.ADS1263_DAC_Test(0, 1)      # Open IN7

                # if (ADC.ADS1263_init_ADC2('ADS1263_ADC2_400SPS') == -1):
                #     exit()
            while (not ADC_Val_buffer.empty()):
                # ADC_Value = ADC.ADS1263_GetAll_ADC2()  # get ADC2 value
                all_sea_val = ADC_Val_buffer.get()
                # logging.info("---- Read Queue Data ----- ")

                for i in [0,1]:
                    if i == 0:
                        # i = sea_serial # IN0 ADC_IN0 Value
                        if (all_sea_val[i] >> 23 == 1):
                            val = REF * 2 - all_sea_val[i] * REF / 0x800000
                            avg = moving_average_a.next(val)
                            # logging.info("---- Logging Info ----:  ADC2 IN%d = -%lf" % (i, (REF * 2 - ADC_Value[i] * REF / 0x800000)))
                            # logging.info(" ---- Logging Info ----:   AVG a : ADC2 IN%d = -%lf" % (i, avg))
                        else:
                            val = all_sea_val[i] * REF / 0x7fffff
                            avg = moving_average_a.next(val)
                            # logging.info("---- Logging Info ----:  ADC2 IN%d = %lf" % (i, (ADC_Value[i] * REF / 0x7fffff)))  # 24bit
                            # logging.info("  ---- Logging Info ----:     AVG a : ADC2 IN%d = %lf" % (i, avg))
                        angle = (360.0 * avg) / REF  # degree of sea
                        
                        theta_sea_t[0] = angle

                        current_pitch_left_buffer.append(angle)
                            # logging.info("---- Logging Info ----:  SEA  ID : {}   - ANGLE : ".format(i)+ format(angle, '.3f'))
                    else:
                            if (all_sea_val[i] >> 23 == 1):
                                val_b = REF * 2 - all_sea_val[i] * REF / 0x800000
                                avg_b = moving_average_b.next(val_b)
                                # logging.info("ADC2 IN%d = -%lf" % (i, (REF * 2 - ADC_Value[i] * REF / 0x800000)))
                                # logging.info("     AVG b : ADC2 IN%d = -%lf" % (i, avg_b))
                            else:
                                val_b = all_sea_val[i] * REF / 0x7fffff
                                avg_b = moving_average_b.next(val_b)
                                # logging.info("ADC2 IN%d = %lf" % (i, (ADC_Value[i] * REF / 0x7fffff)))  # 24bit
                                # logging.info("     AVG b : ADC2 IN%d = %lf" % (i, avg_b))
                            
                            angle_b = (360.0 * avg_b) / REF  # degree of sea

                            theta_sea_t[1] = angle_b
                            current_pitch_right_buffer.append(angle_b)
                        
                    # write sea data 
                    cnt = cnt+1
                    # logging.info("SEA  ID : {}   - ANGLE : ".format(i)+ format(angle_b, '.3f'))
                    if(cnt == 1000 and call_times <= 5):
                        call_times = call_times+1
                        write_sea_data(current_pitch_left_buffer, sea_serial= 0, call_times=call_times)
                        write_sea_data(current_pitch_right_buffer, sea_serial= 1, call_times=call_times)
                        current_pitch_left_buffer.clear()
                        current_pitch_right_buffer.clear()
                        logging.info("---- Logging Info ----:   SEA  data has write , function callback :  {}  Times!  ".format(call_times))
                        cnt = 0
                    time.sleep(1 / frequency)
        else:

                get_sea_val()

            # except IOError as e:
            #     logging.error(e)

            # except KeyboardInterrupt:
            #     logging.info("---- Logging Interrupt----:  ctrl + c: Program end")
            #     # logging.info(current_yaw_right_buffer,current_pitch_right_buffer)
            #     logging.info("---- Logging Interrupt ----:  sea_serial NOT in DefaultSEAChannelSer: threadName.exit() !")
            #     ADC.ADS1263_Exit()
            #     threadName.exit()
            #     exit()


        # elif sea_serial ==1:  # channel 1
        #     cnt =  0
        #     call_times = 0
        #     try:
        #         ADC = ADS1263.ADS1263()
        #         if (ADC.ADS1263_init_ADC1('ADS1263_7200SPS') == -1):
        #             exit()
        #         ADC.ADS1263_SetMode(0)

        #         # ADC.ADS1263_DAC_Test(1, 1)      # Open IN6
        #         # ADC.ADS1263_DAC_Test(0, 1)      # Open IN7

        #         if (ADC.ADS1263_init_ADC2('ADS1263_ADC2_400SPS') == -1):
        #             exit()
        #         while (1):
        #             ADC_Value = ADC.ADS1263_GetAll_ADC2()  # get ADC2 value
        #             i = sea_serial # IN0 ADC_IN0 Value
        #             if (ADC_Value[i] >> 23 == 1):
        #                 val_b = REF * 2 - ADC_Value[i] * REF / 0x800000
        #                 avg_b = moving_average_b.next(val_b)
        #                 # logging.info("ADC2 IN%d = -%lf" % (i, (REF * 2 - ADC_Value[i] * REF / 0x800000)))
        #                 # logging.info("     AVG b : ADC2 IN%d = -%lf" % (i, avg_b))
        #             else:
        #                 val_b = ADC_Value[i] * REF / 0x7fffff
        #                 avg_b = moving_average_b.next(val_b)
        #                 # logging.info("ADC2 IN%d = %lf" % (i, (ADC_Value[i] * REF / 0x7fffff)))  # 24bit
        #                 # logging.info("     AVG b : ADC2 IN%d = %lf" % (i, avg_b))
                    
        #             angle_b = (360.0 * avg_b) / REF  # degree of sea

        #             current_pitch_right_buffer.append(angle_b)
        #             cnt = cnt+1

        #             # logging.info("SEA  ID : {}   - ANGLE : ".format(i)+ format(angle_b, '.3f'))
        #             if(cnt == 1000 and call_times <= 5):
        #                 call_times = call_times+1
        #                 write_sea_data(current_pitch_right_buffer, sea_serial= 1,call_times=call_times)
        #                 current_pitch_right_buffer.clear()
        #                 logging.info("---- Logging Info ----:   SEA  data has write , function callback :  {}  Times!  ".format(call_times))
        #                 cnt = 0
        #             time.sleep(0.1 / frequency)

        #     except IOError as e:
        #         logging.error(e)

        #     except KeyboardInterrupt:
        #         logging.info("---- Logging Info ----:  ctrl + c: Program end")logging.info("---- Get SEA data Success !----- ")
        #         logging.info("---- Logging Info ----:  sea_serial NOT in DefaultSEAChannelSer: threadName.exit() !")
        #         ADC.ADS1263_Exit()
        #         threadName.exit()
        #         exit()

    
    else:
        logging.error("%s: %s, ERROR : sea_serial NOT in DefaultSEAChannelSer !" 
                        % (threadName, time.ctime(time.time())))
        logging.info("---- Logging Info ----:  sea_serial NOT in DefaultSEAChannelSer: threadName.exit() !")
        threadName.exit()
        

def write_sea_data(sea_state_list , sea_serial = 0, call_times = 1):
        # write the sea data
        file_sea_angle = "contact_force_sea_records_"+str(sea_serial)+str(call_times)+".txt"
        with open(file_sea_angle, 'w') as f:
            if sea_serial == 0 and sea_state_list != []:
                f.write("time_stamped"+"\t"+"   left_shoulder_pitch_angle      "+"\n") 
                for frame in sea_state_list:
                    # data file write as txt
                    f.write("   {}     {}  ".format(time.ctime(time.time()), frame) + '\r' )
            if sea_serial == 1 and sea_state_list  != []:
                f.write("time_stamped"+"\t"+"right_shoulder_pitch_angle      "+"\n") 
                for frame in sea_state_list:
                    # data file write as txt
                    f.write("   {}     {}  ".format(time.ctime(time.time()), frame ) + '\r')

# 增加关节舵机角度的vel和iterval参数动态调整接口API
def generate_pose_vtc(angle, servo_id, vel=100, iterval=5000, t_acc=35, t_dec=35, power=10000, mean_dps=70.0):
    pose_tmp = {'angle': angle, 'is_mturn': False, 'interval': iterval,
                'vel': vel, 't_acc': t_acc,
                't_dec': t_dec, 'power': power,
                'mean_dps': mean_dps}
    gen_pose = Pose(POSE=pose_tmp, SERVO_ID=servo_id)
    return gen_pose


class MovingAverage:
    def __init__(self, size: int):
        self.que = queue.Queue(size)

    def next(self, val: float):
        if (self.que.full()):
            self.que.get()  # get the queue item value
        self.que.put(val)
        return sum(self.que.queue) / self.que.qsize()  # calculate the average voltage value


def main():
    # main function to construct new control and records threading

    # dual_arm class obj init
    dual_arm = ArmMotionControl(arm_name="dual_arm", dof=6, CONTROL_MODE="DAMPING", with_wrist=False, is_debug=False,
                                damping_power=1000)
    wrist_gripper = WristGripperControl(servo_channel=ending_servo_idx, is_debug=False)

    # 初始化舵机管理器类对象、串口端口地址
    dual_arm.creat_uservo_obj(SERVO_BAUDRATE=115200)
    if dual_arm.servo_buffer(is_scan_servo=True):
        dual_arm.logging_arm_info()
    # dual arm and gripper init
    dual_arm.arm_state_init(force_angle_init=False)
    dual_arm.set_all_servo_mode(CONTROL_MODE="CIRCLE")
    wrist_gripper.all_ending_init(motion_group=4)


    # dual_arm再现
    wrist_gripper.set_servo_angle(servo_idx=0, angle=DUAL_ARM_TARGET_POSE[3])
    time.sleep(SHORT_TIME_DELAY)
    # 右臂手腕手爪位置伺服，2（IO12）-》it-7，3(IO13)->it-[9]
    wrist_gripper.set_servo_angle(servo_idx=2, angle=DUAL_ARM_TARGET_POSE[7])
    
    time.sleep(4)
    wrist_gripper.set_servo_angle(servo_idx=1, angle=DUAL_ARM_TARGET_POSE[8])
    wrist_gripper.set_servo_angle(servo_idx=3, angle=DUAL_ARM_TARGET_POSE[9])

    time.sleep(3)

    # 创建新线程
    controlThreadName = "control"
    recordThreadName = "record"
    seaRcd0ThreadName = "record_sea_0"
    seaRcd1ThreadName = "record_sea_1"

    # Multi Threading Thread ID [1,2,3,4]
    thread1 = dfrThread(threadID=1,
                        name=controlThreadName,
                        controlPeriod=CONTROL_PERIOD.get("SEC_1.8"),
                        samplingFrequency=SAMPLING_FREQUENCY,
                        dual_arm_obj=dual_arm,
                        wrist_gripper=wrist_gripper)

    thread2 = dfrThread(threadID=2,
                        name=recordThreadName,
                        controlPeriod=None,
                        samplingFrequency=SAMPLING_FREQUENCY,
                        dual_arm_obj=dual_arm)

    thread_sea_0 = dfrThread(threadID = 3,
                        name=seaRcd0ThreadName,
                        controlPeriod=None,
                        samplingFrequency=3*SAMPLING_FREQUENCY,
                        dual_arm_obj=None,
                        sea_serial= 0 )

    thread_sea_1 = dfrThread(threadID = 4,
                        name=seaRcd1ThreadName,
                        controlPeriod=None,
                        samplingFrequency=SAMPLING_FREQUENCY,
                        dual_arm_obj=None,
                        sea_serial=1)

    # 开启新线程
    logging.info("System and thread SEA START  after 3 SEC!")
    thread_sea_0.start()   #  SEA 0 -  read threads
    time.sleep(3)

    thread_sea_1.start()    #SEA 1 - records threads
    time.sleep(1)

    thread1.start()
    time.sleep(2)

    thread2.start()

    # 等待线程完成
    thread1.join()
    thread2.join()
    thread_sea_0.join()
    thread_sea_1.join()

    logging.info("退出主线程")


# main function
if __name__ == '__main__':
    # 创建一个logger_debug
    import sys

    logger = logging.getLogger()
    logger.setLevel(logging.DEBUG)  # Log等级总开关-debug
    print("Logging Set Info: logger.setLevel(logging.DEBUG)  # Log等级总开关-debug")
    try:
        main()
    except KeyboardInterrupt:
        print("Shutting down robot arm debug.")